#ifndef MLTOOL_OPT_H
#define MLTOOL_OPT_H

#include<limits>
#include<cmath>
#include<functional>
#include<algorithm>
#include"vector.h"
#include"operations.h"

namespace math{

typedef double Float;
static const Float Inf = std::numeric_limits<Float>::infinity();
static const Float epsilon = std::numeric_limits<Float>::epsilon();

inline bool almost_equal(Float x, Float y, int ulp = 2)
{
	return std::abs(x - y) <= std::max(std::abs(x), std::abs(y))
							  * epsilon * ulp;
}

inline bool almost_zero(Float x, int ulp = 10)
{
	return std::abs(x) <= epsilon * ulp;
}

typedef vector<Float> vector_type;
typedef Float (*object_type) (const vector_type &);
typedef vector_type (*gradient_type) (const vector_type &);


/* inexat line search */
/*----------------------------------------------
  参数：
  f ------ 目标函数
  diff ------ 目标函数的梯度函数
  x0 ------ 搜索点
  d0 ------ 搜索方向
  f0 ------ 搜索点处的函数值
  df0 ------ 搜索点处的梯度
  t ------ 初始搜索步长
  c1 ------ Armoji条件的系数 0 < c1 < 0.5
  c2 ------ Wolfe 条件的系数 c1 < c2 < 1
  rho ------ 当步长过小时，伸长步长的倍数
  beta ------ backtracking中的缩短系数
  b ------ 搜索区间的右端点
  --------------------------------------------------*/
typedef Float (*search_method) (const object_type &f,
		const gradient_type &diff,
		vector_type& x0,
		vector_type& d0,
		Float &f0,
		vector_type& df0,
		Float t,
		Float c1,
		Float c2,
		Float rho,
		Float beta,
		Float b);

/* Armijo-Goldstein method */
Float goldstein(const object_type &f,       /* in */
				const gradient_type &diff,  /* in */
				vector_type & x0,             /* in/out */ 
				vector_type & d0,             /* in/out */
				Float &f0,                     /* in/out */
				vector_type & df0,           /* in/out */
				Float t = 1,
				Float c1 = 1e-4,
				Float c2 = 0.9,
				Float rho = 2,
				Float beta = 0.5,
				Float b = Inf)
{
	Float a = 0;
	vector_type xt = x0;
	Float delta = t;
	Float ft;
	Float dd0 = inner_prod(df0, d0); //方向导数
	while(true)
	{
		xt = x0 + d0 * t;
		ft = f(xt);
		if(ft > f0 + c1 * t * dd0)  // Armoji条件
		{
			b = t;
			t = (a + b) / 2;
		}
		else if(ft < f0 + (1 - c1) * t * dd0)
		{
			a = t;
			if(std::isfinite(b))
				t = (a + b) / 2;
			else
				t *= rho;
		}
		else
		{
			f0 = ft;
			x0.swap(xt);
			df0 = diff(x0);
			return t;
		}
	}
}

/* Wolfe-Powell method */
Float wolfe(const object_type &f,       /* in */
			const gradient_type &diff,  /* in */
			vector_type & x0,             /* in/out */ 
			vector_type & d0,             /* in/out */
			Float &f0,                     /* in/out */
			vector_type & df0,           /* in/out */
			Float t = 1,
			Float c1 = 0.1,
			Float c2 = 0.5,
			Float rho = 2,
			Float beta = 0.5,
			Float b = Inf)
{
	Float a = 0;
	vector_type xt, dft;
	Float ft;
	Float ddt, dd0 = inner_prod(d0, df0); //方向导数
	while(true)
	{
		xt = x0 + d0 * t;
		ft = f(xt);
		dft = diff(xt);
		ddt = inner_prod(d0, dft);

		if(ft > f0 + c1 * t * dd0)
		{
			b = t;
			t = (a + b) / 2;
		}
		else if(ddt < c2 * dd0)
		{
			a = t;
			if(std::isfinite(b))
				t = (a + b) / 2;
		
			else
				t *= rho;
		}
		else
		{
			x0.swap(xt);
			f0 = ft;
			df0.swap(dft);
			return t;
		}
	}
}

/* strong wolfe conditions */
Float strongwolfe(const object_type &f,       /* in */
			const gradient_type &diff,  /* in */
			vector_type & x0,             /* in/out */ 
			vector_type & d0,             /* in/out */
			Float &f0,                     /* in/out */
			vector_type & df0,           /* in/out */
			Float t = 1,
			Float c1 = 0.1,
			Float c2 = 0.5,
			Float rho = 2,
			Float beta = 0.5,
			Float b = Inf)
{
	Float a = 0;
	vector_type xt, dft;
	Float ft;
	Float ddt, dd0 = inner_prod(d0, df0); //方向导数
	while(true)
	{
		xt = x0 + d0 * t;
		ft = f(xt);
		dft = diff(xt);
		ddt = inner_prod(d0, dft);

		if(ft > f0 + c1 * t * dd0)
		{
			b = t;
			t = (a + b) / 2;
		}
		else if(std::abs(ddt) > c2 * std::abs(dd0))
		{
			a = t;
			if(std::isfinite(b))
				t = (a + b) / 2;
		
			else
				t *= rho;
		}
		else
		{
			x0.swap(xt);
			f0 = ft;
			df0.swap(dft);
			return t;
		}
	}
}
/*backtracking method*/
Float backtrack(const object_type &f,       /* in */
			const gradient_type &diff,  /* in */
			vector_type & x0,             /* in/out */ 
			vector_type & d0,             /* in/out */
			Float &f0,                     /* in/out */
			vector_type & df0,           /* in/out */
			Float t = 1,
			Float c1 = 0.1,
			Float c2 = 0.5,
			Float rho = 2,
			Float beta = 0.5,
			Float b = Inf)
{

	vector_type xt;
	Float ft;
	Float dd0 = inner_prod(df0, d0);
	while(true)
	{
		xt = x0 + d0 * t;
		ft = f(xt);
		if(ft > f0 + c1 * t * dd0)
			t *= beta;

		else
		{
			f0 = ft;
			x0.swap(xt);
			df0 = diff(x0);
			return t;
		}

	}
}


class opt
{
public:
	opt():
		_c1(0.1), _c2(0.2), _t(1), _b(Inf), _rho(2), _beta(0.2)
	{}
	void set_t(Float t0){_t = t0;}
	void set_b(Float b0){_b = b0;}
	void set_c1(Float c1){_c1 = c1;}
	void set_c2(Float c2){_c2 = c2;}
	void set_rho(Float rho){_rho = rho;}
	void set_beta(Float beta){_beta = beta;}

	Float get_c1() const{return _c1;}
	Float get_c2() const{return _c2;}
	Float get_rho() const{return _rho;}
	Float get_beta() const{return _beta;}
	Float get_b() const{return _b;}
	Float get_t() const{return _t;}

	/*gradient descent method*/
	void gradDescent(const object_type &f,
					const gradient_type &diff,
					vector_type &x0,
					search_method line_search)
	{
		vector_type df0 = diff(x0);
		Float f0 = f(x0);
		vector_type d;
		while(!almost_zero(norm_2(df0)))
		{
			d = -df0;
			line_search(f, diff, x0, d,
					    f0, df0, _t, _c1, _c2, 
						_rho, _beta, _b);
		}

		opt_point.swap(x0);
		opt_value = f0;
	}

	/* Fletcher-Reeves conjugate gradient method*/
	void FRCG(const object_type &f,
			  const gradient_type &diff,
			  vector_type &x0,
			  search_method line_search)
	{
		vector_type df0 = diff(x0);
		Float f0 = f(x0);
		vector_type d = -df0;
		Float l0 = inner_prod(df0, df0);
		Float l1;
		while(!almost_zero(norm_2(df0)))
		{
			line_search(f, diff, x0, d,
					    f0, df0, _t, _c1, _c2, 
						_rho, _beta, _b);

			l1 = inner_prod(df0, df0);
			d *= (l1 / l0);
			d -= df0;
			l0 = l1;
		}

		opt_point.swap(x0);
		opt_value = f0;
	}

	/* Polak-Ribiere-Polyak conjugate gradient method*/
	void PRPCG(const object_type &f,
			  const gradient_type &diff,
			  vector_type &x0,
			  search_method line_search)
	{
		vector_type df0 = diff(x0);
		vector_type dft(df0);
		Float f0 = f(x0);
		vector_type d = -df0;
		Float l0 = inner_prod(df0, df0);
		Float l1, l2;
		while(!almost_zero(norm_2(df0)))
		{
			line_search(f, diff, x0, d,
					    f0, dft, _t, _c1, _c2, 
						_rho, _beta, _b);

			l1 = inner_prod(dft, dft);
			l2 = inner_prod(df0, dft);
			d *= (l1 - l2) / l0;
			d -= dft;
			df0 = dft;
			l0 = l1;
		}

		opt_point.swap(x0);
		opt_value = f0;
	}
			
	vector_type opt_point;
	Float opt_value;

private:
	Float _c1;
	Float _c2;
	Float _b;
	Float _t;
	Float _rho;
	Float _beta;
};

}//math
#endif
